#include "robotcalc.h"

RobotCalc::RobotCalc():
    mlinkCnt(0)
{

}

vector<Matrix44f> RobotCalc::ForWard(vector<Matrix14f> theta)
{
    return vector<Matrix44f>();
}
vector<Matrix44f> RobotCalc::Inverse(vector<Matrix14f> theta)
{
    return vector<Matrix44f>();
}
vector<Matrix44f> RobotCalc::Jacobi()
{
    return vector<Matrix44f>();
}

int RobotCalc::AddLink(float theta , float a , float d, float alpha, LinkType type)
{
    TDhStruct link;
    link.a =a;
    link.theta = theta;
    link.d = d;
    link.alpha = alpha;
    link.type = type;
    this->mLinks.push_back(link);
    this->mlinkCnt++;
    return 0;
}

